//You can include any C libraries that you normally use
#include "math.h"
#include "mex.h"   //--This one is required
#include "matrix.h"

#define himg0(i,j,k) himg0[(i+j*ix+k*ix*iy)] 
#define himg(i,j,k) himg[(i+j*lenx+k*lenx*leny)] 

#define img0(i,j,k) img0[(i+j*ix+k*ix*iy)] 
#define img(i,j,k) img[(i+j*lenx+k*lenx*leny)]
#define newimg(i,j,k) newimg[(i+j*ix+k*ix*iy)]
#define tempimg(i,j,k) tempimg[(i+j*ix+k*ix*iy)]

#define newimg0(i,j) newimg0[i+j*ix]
#define newimg1(i,j) newimg1[i+j*ix]
#define newimg2(i,j) newimg2[i+j*ix]

#define gimg(i,j) gimg[i+j*ix]
#define out(i,j) out[i+j*ix]

double HistDistance(double* img, int xstart, int xend, int ystart, int yend, double rm, double gm, double bm, 
        double rstd, double gstd, double bstd, int lenx, int leny)
{                   
    int i, j; 
        
    double r, g, b, r1, g1, b1;
    double dist, d1, d2, d3;                 
    
    double totalpixels = abs(xend-xstart)*abs(yend-ystart);
    
    dist = 10000;
    
    if(totalpixels > 0)
    {
        
           
        for(i = xstart; i < xend; i++)
        {
            for(j = ystart; j < yend; j ++)
            {                       
                r = (int)img(i,j,0);
                g = (int)img(i,j,1);
                b = (int)img(i,j,2);     
                                                    
                r1 += r;
                g1 += g;
                b1 += b;
            }
        }
        
        
        // calculate center of cluster (= mean of each color)
        r1 = r1/totalpixels;
        g1 = g1/totalpixels;
        b1 = b1/totalpixels;  
    
        d1 = rm-r1;
        d2 = gm-g1;
        d3 = bm-b1;
        
        if(fabs(d1) < rstd && fabs(d2) < gstd && fabs(d3) < bstd)
        {    
            dist = sqrt(d1*d1+d2*d2+d3*d3);             
        }
    }
    
    return dist;
}





void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
               
    double* img0 = mxGetPr(prhs[0]); //template
    double* img = mxGetPr(prhs[1]); //main image

    double rm = (double)(mxGetScalar(prhs[2])); 
    double gm = (double)(mxGetScalar(prhs[3])); 
    double bm = (double)(mxGetScalar(prhs[4])); 

    double rstd = (double)(mxGetScalar(prhs[5])); 
    double gstd = (double)(mxGetScalar(prhs[6])); 
    double bstd = (double)(mxGetScalar(prhs[7])); 


    const int *sz = mxGetDimensions(prhs[0]); 
    int ix = sz[0];
    int iy = sz[1];
    int iz = sz[2];

    const int *szmain = mxGetDimensions(prhs[1]); 
    int lenx = szmain[0];
    int leny = szmain[1];

    int x, y, x1, x2, y1, y2;

    int x1best = 0;
    int x2best = 0;
    int y1best = 0;
    int y2best = 0;

    double bestscore;
    double score;

    double r,g, b, r0, g0, b0;
    double rt, gt, bt;
    int i, j, k;

    double d1, d2, d3, dist, hdist;

    int lenx2, leny2, npixels, maxhdist;

    int bestremoved, nremoved, nstep;
   
    double besth = 10000;
    double c = 0.25;
    
    double div = ix*iy;

    y1 = 0;
    bestremoved = 0; 
    nremoved = 0;
    bestscore = ix*iy*iz*10000;

    nstep = 1;
    maxhdist = 50;

    lenx2 = lenx - ix - 1;
    leny2 = leny - iy - 1;    

    while(y1 < leny2) //scan through image with template: leny2 = ylength of image - ylength of template
    {
        x1 = 0;     
        
        while(x1 < lenx2) //lenx2 = xlength of image - xlength of template
        {                                                                                      
            x2 = x1+ix;
            y2 = y1+iy;
            
            dist = 0;
        
            rt = 0;
            gt = 0;
            bt = 0;
       
            for(x = 0; x < ix; x++) //for each point in template subtract from image: ix = xlength of template
            {                
                for(y = 0; y < iy; y++) //iy = ylength of template
                {                 
                    i = x+x1; 
                    j = y+y1;                                                                     
                    
                    r = img(i,j,0);        
                    g = img(i,j,1);        
                    b = img(i,j,2);                                                                
        
                    r0 = img0(x,y,0);        
                    g0 = img0(x,y,1);
                    b0 = img0(x,y,2);                                       
                                                                   
                    d1 = r0-r;
                    d2 = g0-g;
                    d3 = b0-b;
                    
                    rt += r;
                    gt += g;
                    bt += b;
                    
                    dist = dist+sqrt(d1*d1+d2*d2+d3*d3);                               
                }
            }                         
            
            rt = rt/div;
            gt = gt/div;
            bt = bt/div;
            
            d1 = rm-rt;
            d2 = gm-gt;
            d3 = bm-bt;
        
            hdist = 10000;                        
            
            if(fabs(d1) < rstd*c && fabs(d2) < gstd*c && fabs(d3) < bstd*c)
            {    
                hdist = sqrt(d1*d1+d2*d2+d3*d3);             
            }            
   
            score = dist;
                                                                      
            if (score < bestscore & hdist < maxhdist)            
            {                                           
                
                 bestscore = score;
                 bestremoved = nremoved;                                
                                    
                 x1best = x1+1;
                 x2best = x1+ix+1;            
            
                 y1best = y1+1;
                 y2best = y1+iy+1;      
                 
                 besth = hdist;
            }
                           
           x1 = x1 + nstep;
        
        }
        
        y1 = y1 + nstep;    
        
    }

    plhs[0] = mxCreateDoubleScalar(x1best);
    plhs[1] = mxCreateDoubleScalar(x2best);
    plhs[2] = mxCreateDoubleScalar(y1best);
    plhs[3] = mxCreateDoubleScalar(y2best);
    plhs[4] = mxCreateDoubleScalar(bestscore);
    plhs[5] = mxCreateDoubleScalar(bestremoved);
    plhs[6] = mxCreateDoubleScalar(besth);
 
    return;
}